clc;clear;close all;

warning('off')
% filed limits [x_min x_max y_min y_max z_min z_max]
field_limits = [0 100 0 100 0 20];
if isfile("robot.mat")
    load("robot.mat");
    [ r_num,state_dim ] = size(robot);            % robot number,state dimensions for each robot
else
    r_num = 3;                                                  % robot number
    state_dim = 3;                                            % state dimensions for each robot
    robot = [ rand(r_num,1)*field_limits(2)/2, rand(r_num,1)*field_limits(4)/2, rand(r_num,1)*field_limits(6) ];
    save robot.mat robot;
end

robot_scatter_size = 10;

f = figure('Name','SimultanuouslyCoverage3D'); hold on;
scatter3(robot(:,1),robot(:,2),robot(:,3),robot_scatter_size,'filled'); grid on;
sc = scatter3(robot(:,1),robot(:,2),robot(:,3),robot_scatter_size,'filled'); grid on;
axis(field_limits);view(3);

% discretizing the x-y plane, 
discretization_num = 50; mesh_size = field_limits(2)/discretization_num;
x_space = linspace(field_limits(1),field_limits(2),discretization_num);
y_space = linspace(field_limits(3),field_limits(4),discretization_num);
[X_points,Y_points] = meshgrid(x_space,y_space);
points_xy = [reshape(X_points,discretization_num^2,1) reshape(Y_points,discretization_num^2,1)];

% density calculation
centers = [20 20; 80 20; 80 80; 20 80; ]; c_index = 1; duration = 700;
d_center = centers(c_index,:);
R = densityFunc(d_center,points_xy);
%integral = sum(R*mesh_size^2,'all')
h = robot(:,end); sensor_para = 20;
[r,dr_dh] = coverageModel(h,sensor_para,'reciprocal');
%

% initial cost
cost = computeObj_vecform(robot,points_xy,R,mesh_size,r)

% lamda_max = 1; lamda_min=0.01; lamda = @(k) (-(lamda_max-lamda_min)/300 *k +1.01);
robot_init = robot;
robot_his = cell(100,1);
robot_his{1} = robot;

for k = 1:duration*4
    if mod(k,duration) == 0
        
        [X,Y,Z] =  computeMeshforPlot(robot,field_limits,200);
        h(c_index) = figure('Name','CoverageAreaBeforeOptimization'); hold on; grid on;
        surf(X,Y,Z); shading interp
        axis(field_limits(1:4));
        clear X Y Z;
        
        c_index = c_index + 1; 
        if c_index <=4 
            d_center = centers(c_index,:);
            R = densityFunc(d_center,points_xy);
        end
    end
    robot1 = robot;

    for i = 1:r_num
        % refine the robot set of multual communication
        s_i = robot(i,:); 
        rel_dis = vecnorm(s_i(1:2) - robot(:,1:2),2,2); 
        r = coverageModel(robot(:,end),sensor_para,'reciprocal');
        robot_i = robot; r1 = rel_dis < r + r(i); r1(rel_dis == 0) = 1;
        robot_i = robot_i .* r1;
        [r,dr_dh] = coverageModel(robot_i(:,end),sensor_para,'reciprocal'); r = r.*r1; dr_dh = dr_dh.*r1;
        % compute the gradient
        delta_u_i = computeGradient(i,robot_i,r,dr_dh,points_xy,mesh_size,R,d_center);
        direction = delta_u_i/norm(delta_u_i,2); 
        robot1(i,:) = robot(i,:) + 0.1* direction';
    end

    robot_his{k+1} = robot1;
    robot = robot1; robot(:,3) = abs(robot(:,3));
    h = robot(:,end); [r,dr_dh]= coverageModel(h,sensor_para,'reciprocal');    
    figure(f); set(sc,'XData', robot(:,1), 'YData',robot(:,2),'ZData',abs(robot(:,3)));
    cost = computeObj_vecform(robot,points_xy,R,mesh_size,r)
end

datafile = 'Results/MultiDensityCase/robot_his.mat';
save(datafile,'robot_his');

%%
clc;clear;close all;
field_limits = [0 100 0 100 0 20];
r_num = 3; d_num = 4; duration = 700;
datafile = 'Results/MultiDensityCase/robot_his.mat';
load(datafile);
stablePos = cell(d_num,1);
for i = 1:d_num
    stablePos{i} = robot_his{duration*(i)+1};
end
stablePos = cell2mat(stablePos);

centers = [20 20; 80 20; 80 80; 20 80; ];
f1 = figure('Name','PDOPdistributionFourPoints'); axis(field_limits(1:4)); hold on;

[X,Y,Z] = computeMeshforPlot(stablePos,field_limits,200);
pcolor(X,Y,Z); shading interp; colormap default; colorbar; view(2);
scatter(centers(:,1),centers(:,2),30,'r','^','filled');

ax = gca;
outerpos = ax.OuterPosition;
ti = ax.TightInset; 
left = outerpos(1) + ti(1);
bottom = outerpos(2) + ti(2);
ax_width = outerpos(3) - ti(1) - ti(3);
ax_height = outerpos(4) - ti(2) - ti(4);
ax.Position = [left bottom ax_width ax_height];
saveas(f1,'PDOPdistributionFourPoints.eps','epsc');

% plot trajectories
robot_int = robot_his{1};
f2 = figure('Name','Trajectories3D'); view(-110,30); axis(field_limits); hold on; grid on;
robot = cell2mat(robot_his);
c = ['r','g','b','k']; scattersize1= 30;scattersize2 = 60;
rang{1} = 1:r_num:length(robot);
rang{2} = 2:r_num:length(robot);
rang{3} = 3:r_num:length(robot);
for i = 1:r_num
    scatter3(robot_int(i,1),robot_int(i,2),abs(robot_int(i,3)),scattersize1,'o',c(i),'filled');
    scatter3(stablePos(i:r_num:length(stablePos),1),stablePos(i:r_num:length(stablePos),2),abs(stablePos(i:r_num:length(stablePos),3)),scattersize2,'v',c(i),'filled');
    scatter3(robot(rang{i},1),robot(rang{i},2),abs(robot(rang{i},3)),1,'.',c(i));
end
ax = gca;
outerpos = ax.OuterPosition;
ti = ax.TightInset; 
left = outerpos(1) + ti(1);
bottom = outerpos(2) + ti(2);
ax_width = outerpos(3) - ti(1) - ti(3);
ax_height = outerpos(4) - ti(2) - ti(4);
ax.Position = [left bottom ax_width ax_height];
saveas(f2,'Trajectories3DFourPoints.eps','epsc');
